#include <WProgram.h>
#include "Commands/Commands.h"
#include "XBee2/XBee2.h"
#include "Servo/Servo.h"
#include "Streaming/Streaming.h"

#define CENTER_STEERING 90
#define SLIGHT_TURN 15
#define MINIMUM_THROTTLE 59
#define NOT_MOVING MINIMUM_THROTTLE-1
#define SLIGHT_LEFT CENTER_STEERING + SLIGHT_TURN
#define SLIGHT_RIGHT CENTER_STEERING - SLIGHT_TURN

#define SERIAL_BIT_RATE 19200

#define THROTTLE_PIN 5
#define STEERING_PIN 6

#define DISTANCE_PIN_LEFT 6
#define DISTANCE_PIN_RIGHT 7

#define ACCELEROMETER_PIN_X 3
#define ACCELEROMETER_PIN_Y 4
#define ACCELEROMETER_PIN_Z 5

#define MAGIC_POINTS_INCREMENT 500
#define MAGIC_POINTS_INCREMENT_XLARGE 2500

#define STEERING_INCREMENT 10

#define STEERING_MINIMUM 0
#define STEERING_MAXIMUM 180

#define IR_DISTANCE_THRESHOLD 1000
#define MAX_DIFFERENCE_OF_DISTANCES 2000

#define SHOCK_THRESHOLD 100

#define ACCELEROMETER_VCC_PIN 12
#define ACCELEROMETER_GROUND_PIN 13

struct DrivingControls {
  int throttle;
  int steering;
};

struct DrivingControls controls = { NOT_MOVING, CENTER_STEERING };

unsigned long magicPoints = 0;

Servo throttle;
Servo steering;

XBee2 xbee;

unsigned int leftBuffer[] = { 0, 0, 0, 0 };
unsigned int rightBuffer[] = { 0, 0, 0, 0 };
unsigned int left, right = 0;
unsigned short distanceCounter = 0;

bool dodging = false;

int x, y, z = 0;
int flat_x, flat_y, flat_z = 0;

void controlCar();
void initializeServos();
void distances();
bool objectDetected();
int dodgeTowards();
void readAccelerometer();
void calibrateAccelerometer();
void printShockStatus();
void printObjectDetectionStatus();
void handleDriverCommand(char command);
bool shock();

void main() __attribute__ ((noreturn));

void powerUp() {
  pinMode(ACCELEROMETER_VCC_PIN, OUTPUT);
  pinMode(ACCELEROMETER_GROUND_PIN, OUTPUT);
  digitalWrite(ACCELEROMETER_VCC_PIN, HIGH);
  digitalWrite(ACCELEROMETER_GROUND_PIN, LOW);
}

void main(void) {
  init();
  powerUp();
  Serial.begin(SERIAL_BIT_RATE);
  xbee.enterApiMode(false);
  xbee >> COORDINATOR_ADDRESS << "D. reset" << endl;
  initializeServos();
  readAccelerometer();
  calibrateAccelerometer();
  for (;;) {
    loop();
  }
}

void loop() {
  xbee.receiveStuff(&handleDriverCommand);
  controlCar();
  readAccelerometer();
  distances();
  if (shock()) {
    printShockStatus();
    calibrateAccelerometer();
  }
}

extern "C" void __cxa_pure_virtual() {
  cli();
  for (;;)
    ;
}

void handleDriverCommand(char command) {
  switch (command) {
  case C_FASTER:
    controls.throttle = min(controls.throttle + 1, 180);
    break;
  case C_SLOWER:
    controls.throttle = max(controls.throttle - 1, NOT_MOVING);
    break;
  case C_MORE_LEFT:
    controls.steering = max(controls.steering - STEERING_INCREMENT, STEERING_MINIMUM);
    break;
  case C_MORE_RIGHT:
    controls.steering = min(controls.steering + STEERING_INCREMENT, STEERING_MAXIMUM);
    break;
  case C_CENTER:
    controls.steering = CENTER_STEERING;
    break;
  case C_SLIGHT_LEFT:
    controls.steering = SLIGHT_LEFT;
    break;
  case C_SLIGHT_RIGHT:
    controls.steering = SLIGHT_RIGHT;
    break;
  case C_MAXIMUM_LEFT:
    controls.steering = STEERING_MINIMUM;
    break;
  case C_MAXIMUM_RIGHT:
    controls.steering = STEERING_MAXIMUM;
    break;

  case C_NOT_MOVING:
    controls.throttle = NOT_MOVING;
    break;
  case C_MINIMUM_THROTTLE:
    controls.throttle = MINIMUM_THROTTLE;
    break;
  case C_MAGIC:
    magicPoints = millis() + MAGIC_POINTS_INCREMENT;
    break;
  case C_MAGIC_XLARGE:
    magicPoints = millis() + MAGIC_POINTS_INCREMENT_XLARGE;
    break;
  case C_DODGE:
    dodging = true;
    xbee >> COORDINATOR_ADDRESS << "dodging" << endl;
    break;
  case C_DONT_DODGE:
    dodging = false;
    xbee >> COORDINATOR_ADDRESS << "not dodging" << endl;
    break;
  case C_PRINT:
    printShockStatus();
    printObjectDetectionStatus();
    break;
  case C_CALIBRATE_ACCELEROMETER:
    calibrateAccelerometer();
    break;
  case C_GO_TO_API_MODE:
    xbee.enterApiMode(true);
    break;
  case C_GO_TO_REGULAR_MODE:
    xbee.enterApiMode(false);
  case C_NEVERMIND:
    break;
  default:
    break;
  }
}

void initializeServos() {
  throttle.attach(THROTTLE_PIN);
  steering.attach(STEERING_PIN);
  controlCar();
}

void controlCar() {
  unsigned long _millis = millis();
  if (controls.throttle >= MINIMUM_THROTTLE && magicPoints > _millis) {
    throttle.write(controls.throttle);
  } else {
    throttle.write(0);
  }
  if (dodging && objectDetected()) {
    int dodgingSteering = constrain(map(dodgeTowards(), -MAX_DIFFERENCE_OF_DISTANCES, MAX_DIFFERENCE_OF_DISTANCES, 0, 180), STEERING_MINIMUM, STEERING_MAXIMUM);
    steering.write(dodgingSteering);
  } else {
    steering.write(controls.steering);
  }
}

bool objectDetected() {
  left = leftBuffer[0] + leftBuffer[1] + leftBuffer[2] + leftBuffer[3];
  right = rightBuffer[0] + rightBuffer[1] + rightBuffer[2] + rightBuffer[3];
  return left > IR_DISTANCE_THRESHOLD || right > IR_DISTANCE_THRESHOLD;
}

void distances() {
  distanceCounter = (distanceCounter + 1) % 4;
  leftBuffer[distanceCounter] = analogRead(DISTANCE_PIN_LEFT);
  rightBuffer[distanceCounter] = analogRead(DISTANCE_PIN_RIGHT);
}

int dodgeTowards() {
  if (objectDetected()) {
    if (right > left) {
      return right * 2 - left;
    } else {
      return right - left * 2;
    }
  }
  return 0;
}

void readAccelerometer() {
  x = analogRead(ACCELEROMETER_PIN_X);
  y = analogRead(ACCELEROMETER_PIN_Y);
  z = analogRead(ACCELEROMETER_PIN_Z);
}

void calibrateAccelerometer() {
  flat_x = x;
  flat_y = y;
  flat_z = z;
}

int relativeX() {
  return (x - flat_x);
}

int relativeY() {
  return (y - flat_y);
}

int relativeZ() {
  return (z - flat_z);
}

bool _shock(int value) {
  return abs(value) > SHOCK_THRESHOLD;
}

bool shock() {
  return _shock(relativeX()) | _shock(relativeY()) || _shock(relativeZ());
}

void printShockStatus() {
  xbee >> COORDINATOR_ADDRESS << "shock!" << " x: " << relativeX() << " y: " << relativeY() << " z: " << relativeZ() << endl;
}

void printObjectDetectionStatus() {
  xbee >> COORDINATOR_ADDRESS << "object detected - left:  " << left << " right: " << right << endl;
}

